#include "huang_onepass.h"
#include "modbus_tcp_user.h"
#include "axis.h"
#include "bsp.h"
#include <string.h>
#include "control.h"
#include "motion_cmd.h"
#include "axis.h"
// static USHORT   *usRegInputBuf;
// static USHORT   *usRegHoldingBuf;
// static BOOL     *usRegCoilsBuf;
// static BOOL     *usRegDiscreteBuf;
#define AXIS_X AXIS1
#define AXIS_CUP AXIS2
#define AXIS_Y AXIS3
static float *x_press_ink_pos;
static float *x_clean_start_pos;
static float *x_clean_end_pos;
static float *cap_press_ink_pos;
static float *cap_clean_pos;
static float *coor_x;
static float *coor_ink;
static float *x_clean_speed;
static float *ink_clean_speed;
static float *print_x_coor;
static float *print_x_speed;

static uint32_t *autocleantime; // 51  ��ʱʱ��
static int32_t pressinktimeout = 0;


static uint32_t autocleantime_now = 0;
static uint8_t VendorAutoClean(void);
static uint8_t WaitActBefMove(uint8_t axis)
{
    if (axis == AXIS_X)
    {
        mt_move_home(AXIS_CUP, axis_info[AXIS_CUP].axis_param.homeSpeed);
        if (axis_info[AXIS_CUP].move_info->move_rsp == MOVE_STOP)
        {
            return 1;
        }
    }
    return 0;
}

static uint8_t VendorInit(void)
{
    // usRegCoilsBuf = modbus_get_coils_buf();
    // usRegDiscreteBuf = modbus_get_discrete_buf();
    // usRegHoldingBuf = modbus_get_holding_buf();
    // usRegInputBuf = modbus_get_input_buf();

    x_press_ink_pos = (float *)&usRegHoldingBuf[31];
    x_clean_start_pos = (float *)&usRegHoldingBuf[33];
    x_clean_end_pos = (float *)&usRegHoldingBuf[35];
    cap_press_ink_pos = (float *)&usRegHoldingBuf[39];
    cap_clean_pos = (float *)&usRegHoldingBuf[41];
    coor_x = (float *)&usRegInputBuf[0];
    coor_ink = (float *)&usRegInputBuf[2];
    x_clean_speed = (float *)&usRegHoldingBuf[37];
    ink_clean_speed = (float *)&usRegHoldingBuf[43];
    print_x_coor = (float *)&usRegHoldingBuf[47];
    print_x_speed = (float *)&usRegHoldingBuf[49];
    autocleantime = (uint32_t *)&usRegHoldingBuf[51];

    g_pump_info[0].pump_input = IN13;
    g_pump_info[0].pump_out = OUT1;
    g_pump_info[0].pump_status = 0;
    g_pump_info[1].pump_input = IN14;
    g_pump_info[1].pump_out = OUT2;
    g_pump_info[1].pump_status = 0;
    g_pump_info[2].pump_input = IN15;
    g_pump_info[2].pump_out = OUT3;
    g_pump_info[2].pump_status = 0;
    g_pump_info[3].pump_input = IN16;
    g_pump_info[3].pump_out = OUT4;
    g_pump_info[3].pump_status = 0;

    g_pump_spill = IN6;
    return 0;
}

static uint8_t OnMainLoop(void)
{
    float pos_acc;
    float speed_acc;
    g_pump_info[0].pump_timeout = usRegHoldingBuf[46];
    g_pump_info[1].pump_timeout = usRegHoldingBuf[46];
    g_pump_info[2].pump_timeout = usRegHoldingBuf[46];
    g_pump_info[3].pump_timeout = usRegHoldingBuf[46];

    usRegInputBuf[16] = g_pump_info[0].pump_status;
    usRegInputBuf[17] = g_pump_info[1].pump_status;
    usRegInputBuf[18] = g_pump_info[2].pump_status;
    usRegInputBuf[19] = g_pump_info[3].pump_status;

    if(action_func == 0)
    {
        if (usRegCoilsBuf[37])
        {
            bsp_set_out(OUT16, GPIO_PIN_SET);
            bsp_set_out(OUT15, GPIO_PIN_SET);
            pressinktimeout = 5000;
    
        }
        else
        {
            bsp_set_out(OUT16, GPIO_PIN_RESET);
            // bsp_set_out(OUT15, GPIO_PIN_RESET);
        }
        pressinktimeout--;
        if(pressinktimeout < 0)
        {
            pressinktimeout = 0;
            bsp_set_out(OUT15, GPIO_PIN_RESET);
        }
    }


    static uint8_t printf_x_bt = 0;
    if (printf_x_bt != usRegCoilsBuf[38]) // X轴去打印位置
    {
        printf_x_bt = usRegCoilsBuf[38];
        if (printf_x_bt)
        {
            memcpy(&pos_acc, print_x_coor, sizeof(float));
            int64_t print_x_pos = (int64_t)(pos_acc / 25.4f * (float)axis_info[AXIS_X].axis_param.resolution);
            memcpy(&speed_acc, print_x_speed, sizeof(float));
            uint32_t speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[AXIS_X].axis_param.resolution);
            mt_moveto_r_comb(AXIS_X, print_x_pos, speed);
            osDelay(5000);
            mt_wait_axis_stop(AXIS_X,0,10000);
            
            bsp_set_out(OUT14, GPIO_PIN_SET);
            bsp_set_out(OUT13, GPIO_PIN_SET);
        }
    }

    static uint8_t set_x_press_ink_pos = 0;
    if (set_x_press_ink_pos != usRegCoilsBuf[32])
    {
        set_x_press_ink_pos = usRegCoilsBuf[32];
        if (set_x_press_ink_pos)
        {
            memcpy(x_press_ink_pos, coor_x, sizeof(float));
        }
    }

    static uint8_t set_x_clean_start_pos = 0;
    if (set_x_clean_start_pos != usRegCoilsBuf[33])
    {
        set_x_clean_start_pos = usRegCoilsBuf[33];
        if (set_x_clean_start_pos)
        {
            memcpy(x_clean_start_pos, coor_x, sizeof(float));
        }
    }

    static uint8_t set_x_clean_end_pos = 0;
    if (set_x_clean_end_pos != usRegCoilsBuf[34])
    {
        set_x_clean_end_pos = usRegCoilsBuf[34];
        if (set_x_clean_end_pos)
        {
            memcpy(x_clean_end_pos, coor_x, sizeof(float));
        }
    }

    static uint8_t set_cap_press_ink_pos = 0;
    if (set_cap_press_ink_pos != usRegCoilsBuf[35])
    {
        set_cap_press_ink_pos = usRegCoilsBuf[35];
        if (set_cap_press_ink_pos)
        {
            memcpy(cap_press_ink_pos, coor_ink, sizeof(float));
        }
    }

    static uint8_t set_cap_clean_pos = 0;
    if (set_cap_clean_pos != usRegCoilsBuf[36])
    {
        set_cap_clean_pos = usRegCoilsBuf[36];
        if (set_cap_clean_pos)
        {
            memcpy(cap_clean_pos, coor_ink, sizeof(float));
        }
    }
    memcpy(&pos_acc, cap_press_ink_pos, sizeof(float));
    axis_info[AXIS_CUP].axis_param.maxLen = (uint64_t)(pos_acc / 25.4f * (float)axis_info[AXIS_CUP].axis_param.resolution);

    uint32_t timeout = *autocleantime * 1000;
    if ((timeout > 0)&&(mt_get_axis_coor(AXIS_X)>axis_info[AXIS_X].axis_param.resolution)*10)
    {
        autocleantime_now++;
        if (autocleantime_now > timeout)
        {
            if (action_func == 0)
            {
                action_func = 100;
                VendorAutoClean();

                autocleantime_now = 0;
                action_func = 0;
            }
        }
    }
    return 0;
}

static uint8_t VendorAutoClean(void)
{
    printf("VendorAutoClean in\n");
    uint8_t flag = 0;
    autocleantime_now = 0;
    if(bsp_get_out(OUT14))
    {
        bsp_set_out(OUT14, GPIO_PIN_RESET);
        bsp_set_out(OUT13, GPIO_PIN_RESET);
        osDelay(12000);
        flag = 1;
    }

    int64_t dest;
    uint32_t speed;
    float pos_acc;
    float speed_acc;
    memcpy(&pos_acc, x_press_ink_pos, sizeof(float));
    memcpy(&speed_acc, x_clean_speed, sizeof(float));
    printf("x_press_ink_pos %f x_clean_speed %f\n", pos_acc, speed_acc);
    dest = (int64_t)(pos_acc / 25.4f * (float)axis_info[AXIS_X].axis_param.resolution);
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[AXIS_X].axis_param.resolution);
    mt_moveto(AXIS_X, dest, speed);
    osDelay(1000);
    memcpy(&pos_acc, cap_clean_pos, sizeof(float));
    memcpy(&speed_acc, ink_clean_speed, sizeof(float));
    dest = (int64_t)(pos_acc / 25.4f * (float)axis_info[AXIS_CUP].axis_param.resolution);
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[AXIS_CUP].axis_param.resolution);
    mt_moveto(AXIS_CUP, dest, speed);
    printf("VendorAutoClean press ink\n");
    osDelay(1000);

    bsp_set_out(OUT16, GPIO_PIN_SET);
    bsp_set_out(OUT15, GPIO_PIN_SET);
    osDelay(usRegHoldingBuf[45]);
    bsp_set_out(OUT16, GPIO_PIN_RESET);
    pressinktimeout = 5000;
    osDelay(5000);
    bsp_set_out(OUT15, GPIO_PIN_RESET);

    osDelay(1000);

    mt_moveto(AXIS_CUP, 0, speed);
    memcpy(&pos_acc, x_clean_start_pos, sizeof(float));
    memcpy(&speed_acc, x_clean_speed, sizeof(float));
    dest = (int64_t)(pos_acc / 25.4f * (float)axis_info[AXIS_X].axis_param.resolution);
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[AXIS_X].axis_param.resolution);
    mt_moveto(AXIS_X, dest, speed);
    printf("VendorAutoClean cleaning\n");

    memcpy(&speed_acc, ink_clean_speed, sizeof(float));
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[AXIS_CUP].axis_param.resolution);
    memcpy(&pos_acc, cap_clean_pos, sizeof(float));
    dest = (int64_t)(pos_acc / 25.4f * (float)axis_info[AXIS_CUP].axis_param.resolution);
    mt_moveto(AXIS_CUP, dest, speed);

    memcpy(&pos_acc, x_clean_end_pos, sizeof(float));
    dest = (int64_t)(pos_acc / 25.4f * (float)axis_info[AXIS_X].axis_param.resolution);
    memcpy(&speed_acc, x_clean_speed, sizeof(float));
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[AXIS_X].axis_param.resolution);
    mt_moveto(AXIS_X, dest, speed);
    osDelay(1000);

    if(flag == 0)
    {
        memcpy(&speed_acc, ink_clean_speed, sizeof(float));
        speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[AXIS_CUP].axis_param.resolution);
        mt_moveto(AXIS_CUP, 0, speed);
    
        memcpy(&speed_acc, x_clean_speed, sizeof(float));
        speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[AXIS_X].axis_param.resolution);
        mt_moveto(AXIS_X, 0, speed);
    
        memcpy(&pos_acc, cap_press_ink_pos, sizeof(float));
        dest = (int64_t)(pos_acc / 25.4f * (float)axis_info[AXIS_CUP].axis_param.resolution);
        memcpy(&speed_acc, ink_clean_speed, sizeof(float));
        speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[AXIS_CUP].axis_param.resolution);
        mt_moveto(AXIS_CUP, dest, speed);

    }
    else
    {
        memcpy(&speed_acc, ink_clean_speed, sizeof(float));
        uint32_t speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[AXIS_CUP].axis_param.resolution);
        mt_moveto(AXIS_CUP, 0, speed);
    
        memcpy(&pos_acc, print_x_coor, sizeof(float));
        int64_t print_x_pos = (int64_t)(pos_acc / 25.4f * (float)axis_info[AXIS_X].axis_param.resolution);
        memcpy(&speed_acc, print_x_speed, sizeof(float));
        speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[AXIS_X].axis_param.resolution);
        mt_moveto(AXIS_X, print_x_pos, speed);
        
        osDelay(3000);
        
        bsp_set_out(OUT14, GPIO_PIN_SET);
        bsp_set_out(OUT13, GPIO_PIN_SET);

    }

    printf("VendorAutoClean out\n");
    return 1;
}

static uint8_t DoWetAction(void)
{
    printf("DoWetAction in\n");
    int64_t dest;
    uint32_t speed;
    float pos_acc;
    float speed_acc;
    memcpy(&speed_acc, x_clean_speed, sizeof(float));
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[AXIS_X].axis_param.resolution);
    mt_moveto(AXIS_X, 0, speed);

    memcpy(&pos_acc, cap_press_ink_pos, sizeof(float));
    dest = (int64_t)(pos_acc / 25.4f * (float)axis_info[AXIS_CUP].axis_param.resolution);
    memcpy(&speed_acc, ink_clean_speed, sizeof(float));
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[AXIS_CUP].axis_param.resolution);
    mt_moveto(AXIS_CUP, dest, speed);
    return 1;
}

static uint8_t MotionReady(void)
{
    DoWetAction();
    return 0;
}

void huang_onepass_init(void)
{
    VendorSpec.VendorInit = VendorInit;
    VendorSpec.WaitActBefMove = WaitActBefMove;
    VendorSpec.OnMainLoop = OnMainLoop;
    VendorSpec.VendorAutoClean = VendorAutoClean;
    VendorSpec.DoWetAction = DoWetAction;
    VendorSpec.MotionReady = MotionReady;
    // VendorSpec.DoOutWetAction = DoWetOutActionDefault;
}
